/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
* The sources are based on the OpenMMM 2010 package by Stefan Gaertner and Martin Do.
*/
#ifndef __MMM_LEGACYC3DREADER_H_
#define __MMM_LEGACYC3DREADER_H_


#include "../../MMMCore.h"
#include "../../MMMImportExport.h"
#include "MarkerMotion.h"
#include "UnlabeledMarkerMotion.h"


#include <string>
#include <map>
#include <Eigen/Core>
#include <fstream>
#include <iostream>

namespace MMM
{
/*!
    \brief This legacy reader parses a Vicon C3D data file and creates a MarkerMotion object.
*/
class MMM_IMPORT_EXPORT LegacyMotionReaderC3D {

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    LegacyMotionReaderC3D();
    virtual ~LegacyMotionReaderC3D();

	/*!
		Opens the file and parses the content to build a MarkerMotion object.
		\param filename The file to read.
        \return In case of failures an empty MarkerMotionPtr is returned.
	*/
    MarkerMotionPtr loadC3D(const std::string filename);

    /*!
        Opens the file and parses the content to build a UnlabeledMarkerMotion object.
        \param filename The file to read.
    */
    UnlabeledMarkerMotionPtr loadUnlabeledC3D(const std::string filename);

    std::string markerPrefix;
	
protected:
	bool validate();

	void readMarkerData(unsigned short marker_num, std::vector<Eigen::Vector3f> &val, Eigen::VectorXf &res);
	void readMarkerData(MarkerMotionPtr data);

	int getNumFrames();

	int getNumMarkers();

	//header properties
	unsigned short num_channels;
	unsigned short first_field;
	unsigned short last_field;
	float scale_factor;
	unsigned short start_record_num;
	unsigned short frames_per_field;
	unsigned char ptype;

	long getFilePositionGroup(const char* group);
	long getFilePositionParams(long group_pos, const char* parameters);

	void Read_C3D_Marker_Labels();

	void Read_C3D_Header();

	void Read_C3D_Marker(unsigned short marker_num, std::vector<Eigen::Vector3f> &val, Eigen::VectorXf &res);

	static void ConvertFloatToDec(float f, char* bytes);
	static float ConvertDecToFloat(char bytes[4]);

	void RPF(int *offset, char *type, char *dim);
	float convertFloat(char mem_float[4]);

	std::vector< std::string > labels;

	//file properties
	std::ifstream infile;
	std::string filename;

	//header properties
	unsigned short num_markers;
	unsigned short num_used_markers;
	float video_rate;
};

typedef boost::shared_ptr<LegacyMotionReaderC3D> LegacyMotionReaderC3DPtr;

}

#endif /* LEGACYC3DReader_H_ */
